#include "ScnsGyro.c.h"

#if defined(SCNS_GYRO_ENABLE)&&SCNS_GYRO_ENABLE==1

#include "ScnsMenu.h"
#include "ScnsSmctpExtOscilloscope.h"
#include "ScnsIcm20602.h"
#include "ScnsIcm42605.h"
#include "ScnsMpu6050.h"
#include "ScnsMpu9250.h"

static uint8 useCalib=0;

static float calibBackUp[4];

ScnsStatus scnsBspGyroInit(ScnsGyroEnum gyrN)
{
#if defined(SCNS_ICM20602_ENABLE)&&SCNS_ICM20602_ENABLE==1
    scnsCheck(ScnsStatus,scnsIcm20602Init(SCNS_ICM20602_1),SCNS_STATUS_OK);
#elif defined(SCNS_ICM42605_ENABLE)&&SCNS_ICM42605_ENABLE==1
    scnsCheck(ScnsStatus,scnsIcm42605Init(SCNS_ICM42605_1),SCNS_STATUS_OK);
#elif defined(SCNS_MPU6050_ENABLE)&&SCNS_MPU6050_ENABLE==1
    scnsCheck(ScnsStatus,scnsMpu6050Init(SCNS_MPU6050_1),SCNS_STATUS_OK);
#elif defined(SCNS_MPU9250_ENABLE)&&SCNS_MPU9250_ENABLE==1
    scnsCheck(ScnsStatus,scnsMpu9250Init(SCNS_MPU9250_1),SCNS_STATUS_OK);
#else
#error
#endif
    static const ScnsMenuExtArray extArray4={.dimSize=1,.dim={4},};
    scnsMenuAdd(4,useCalib,,SCNS_TYPE_BOOLEAN,1,2,1,1);
    scnsMenuAddArray(4,calibBackUp,,SCNS_TYPE_FLOAT,&extArray4,0,2,0,0);
    return SCNS_STATUS_OK;
}

ScnsStatus scnsBspGyroPostInit(ScnsGyroEnum gyrN)
{
    scnsMenuSetPage(4,"gyro",NULL);
    scnsMenuAddConstStruct(4,scnsGyroResult[SCNS_GYRO_1],pitch,,SCNS_TYPE_FLOAT);
    scnsMenuAddConstStruct(4,scnsGyroResult[SCNS_GYRO_1],roll,,SCNS_TYPE_FLOAT);
    scnsMenuAddConstStruct(4,scnsGyroResult[SCNS_GYRO_1],yaw,,SCNS_TYPE_FLOAT);
    scnsMenuAddConstStruct(4,scnsGyroResult[SCNS_GYRO_1],temp,,SCNS_TYPE_FLOAT);
    return SCNS_STATUS_OK;
}

void scnsBspGyroResultUpdate(ScnsGyroEnum gyrN)
{
#if defined(SCNS_SMCTP_EXT_OSCILLOSCOPE_ENABLE)&&SCNS_SMCTP_EXT_OSCILLOSCOPE_ENABLE==1
    ScnsGyroResult this=scnsGyroResult[gyrN];
    if(scnsMenuCanPageSend(4))
        scnsSmctpExtOscilloscopeSendFloat16(
            SCNS_SMCTP_CHANNEL_VISUAL_SCOPE,0,
            this.pitch,
            this.roll,
            this.yaw,

            this.dPitch,
            this.oDPitch,

            this.dRoll,
            this.oDRoll,

            this.dYaw,
            this.oDYaw,

            this.oAx,
            this.oAy,
            this.oAz,

            this.ax,
            this.ay,
            this.az,

            this.pAx,
            this.pAy,
            this.pAz,
        );
#endif
}

ScnsGyroSource scnsBspGyroRead(ScnsGyroEnum gyrN)
{
#if defined(SCNS_ICM20602_ENABLE)&&SCNS_ICM20602_ENABLE==1
    ScnsGyroSource src=scnsIcm20602Read(SCNS_ICM20602_1);
#elif defined(SCNS_ICM42605_ENABLE)&&SCNS_ICM42605_ENABLE==1
    ScnsGyroSource src=scnsIcm42605Read(SCNS_ICM42605_1);
#elif defined(SCNS_MPU9250_ENABLE)&&SCNS_MPU9250_ENABLE==1
    ScnsGyroSource src=scnsMpu9250Read(SCNS_MPU9250_1);
#elif defined(SCNS_MPU6050_ENABLE)&&SCNS_MPU6050_ENABLE==1
    ScnsGyroSource src=scnsMpu6050Read(SCNS_MPU6050_1);
#else
#error
#endif
    return src;
}

uint8 scnsBspGyroIsCalibrationOk(ScnsGyroEnum gyrN,const double DX[9])
{
    return scnsMpu9250IsCalibrationOk(DX);
}

void scnsBspGyroCalibrationBackup(ScnsGyroEnum gyrN,const ScnsGyroSourceCalibration*ca)
{
    hxPrintfFL("Backup gyroBiasX:%f,gyroBiasY:%f,gyroBiasZ:%f,zeroA:%f\n",ca->gyroBiasX,ca->gyroBiasY,ca->gyroBiasZ,ca->zeroA);
    calibBackUp[0]=ca->gyroBiasX;
    calibBackUp[1]=ca->gyroBiasY;
    calibBackUp[2]=ca->gyroBiasZ;
    calibBackUp[3]=ca->zeroA;
    useCalib=1;
}

ScnsStatus scnsBspGyroCalibrationLoad(ScnsGyroEnum gyrN,ScnsGyroSourceCalibration*ca,float q[4][1])
{
    scnsMenuLoadAll();
    if(useCalib)
    {
        ca->gyroBiasX=calibBackUp[0];
        ca->gyroBiasY=calibBackUp[1];
        ca->gyroBiasZ=calibBackUp[2];
        ca->zeroA=calibBackUp[3];
        hxPrintfFL("Load gyroBiasX:%f,gyroBiasY:%f,gyroBiasZ:%f,zeroA:%f\n",ca->gyroBiasX,ca->gyroBiasY,ca->gyroBiasZ,ca->zeroA);
        return SCNS_STATUS_OK;
    }
    else
    {
        return SCNS_STATUS_FAILED;
    }
}

#endif
